/***************************************************************************
 创建者: 华磊
 开始时间: 2019.11.7
 copyright: (C) 华友高科
 ***************************************************************************/
#include "wavemotion.h"
#include "qdebug.h"
//#include <frames.hpp>
//#include <frames_io.hpp>
//#include <trajectory_composite.hpp>
//#include <trajectory.hpp>
//#include <trajectory_segment.hpp>
//#include <trajectory_stationary.hpp>
//#include <trajectory_composite.hpp>
//#include <velocityprofile_trap.hpp>
//#include <path_roundedcomposite.hpp>
//#include <rotational_interpolation_sa.hpp>
//#include <velocityprofile_spline.hpp>
//#include <velocityprofile_rect.hpp>
//#include <utilities/error.h>
//#include <path_circle.hpp>
//#include <Eigen/Dense>
//#include "path_3bezier.h"
//#include "velocityprofile_improvedspline.h"
#include "axisobject.h"


WaveMotion::WaveMotion(double deltaTimeIn, MotionMessage *motionMessageIn, double maxLineVelIn, double maxAccIn)
{
    deltaTime=deltaTimeIn;
    motionMessage=motionMessageIn;
    timeAxisAcceleration=maxAccIn;
    maxLineVel=maxLineVelIn;

    maxLineVel_ratio=1;
    timeAxisAcceleration_ratio=1;

    accumulatedTime=0;
    weaveCondition.motionStyle=E_WAVE_TYPE_NONE;
    weaveCondition.frequency=1;
    weaveCondition.width=0;
    timeAxisScale=0;
    basicStatus=E_WAVE_STATUS_STOPED;
    initialSpeedRatioAxis();
}

E_WAVE_STATUS WaveMotion::getWaveMotionStatus()
{
    return basicStatus;
}


int WaveMotion:: initialSpeedRatioAxis()
{
    //轴初始化
    JointAttribute attribute;
    //trap用
    attribute.maxAccelation=timeAxisAcceleration;
    //attribute.maxVelocity=1.7;
    attribute.maxVelocity=maxLineVel;
    //spline用
//        attribute.maxAccelation=6;
//        attribute.maxVelocity=2;

    attribute.maxTorque=10000;
    attribute.maxPosition=1000.1;
    attribute.minPosition=-0.1;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100;
    attribute.externAxisFlag=0;
    attribute.gearRatio=1;
    attribute.jointName="waveMotionAxis";
    attribute.mobile=false;
    attribute.processFollowError=20000;
    attribute.screwPitch=1;
    attribute.targetFollowError=10000;
    attribute.axisLogicalType=E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE;

    speedRatioAxis=new AxisObject(1,attribute,motionMessage);
    speedRatioAxis->axisIdByRobot=9800;
    speedRatioAxis->deltaTime=deltaTime;

    return 1;
}

//int WaveMotion::setTimeAxisScale(double ratio)
//{
//    timeAxisScale=ratio;
//    return 1;
//}

int WaveMotion::setWaveType(E_WAVE_TYPE typeIn, double widthIn, double frequencyIn)
{
    weaveCondition.motionStyle=typeIn;
    weaveCondition.width=widthIn;
    weaveCondition.frequency=frequencyIn;
    return 1;
}

int WaveMotion::setWaveMotionInfo(WeaveMotionCondition infoIn)
{
    weaveCondition=infoIn;
    return 1;
}

int WaveMotion::setWaveAcceleration(double accelerationIn, double maxVelIn,bool isRatio)
{
    if(isRatio)
    {
        timeAxisAcceleration_ratio=accelerationIn;
        maxLineVel_ratio=maxVelIn;
    }
    else
    {
        timeAxisAcceleration=accelerationIn;
        maxLineVel=maxVelIn;
    }


    speedRatioAxis->setMaxAcceleration(timeAxisAcceleration*timeAxisAcceleration_ratio);
    speedRatioAxis->setMaxVel(maxLineVel*maxLineVel_ratio);
    return 1;
}

int WaveMotion::startWave()
{
    if(E_WAVE_TYPE_NONE==weaveCondition.motionStyle)
    {
        return 0;
    }
    haltStage=0;
    stopCount_left=0;
    stopCount_right=0;
    theaAxisTime=0;
    accumulatedTime=0;
    haltTimeCount=0;
    speedRatioAxis->positionCommand=0.0;
    speedRatioAxis->positionCommandOld=0.0;
    speedRatioAxis->positionFeedback=0.0;
    speedRatioAxis->clearTraj();//保证立刻覆盖前面的旧轨迹
    speedRatioAxis->trapMove(weaveCondition.width/2.0,1);

    basicStatus=E_WAVE_STATUS_TORUN;
    return 1;

}

int WaveMotion::stopWave()
{
    if(E_WAVE_TYPE_NONE==weaveCondition.motionStyle)
    {
        return 0;
    }
    speedRatioAxis->clearTraj();//保证立刻覆盖前面的旧轨迹
    speedRatioAxis->trapMove(0,1);
    basicStatus=E_WAVE_STATUS_TOSTOP;

//    basicStatus=E_WAVE_STATUS_STOPED;
    return 1;

}

int WaveMotion::ceaseWave()
{
    if(E_WAVE_TYPE_NONE==weaveCondition.motionStyle)
    {
        return 0;
    }
    speedRatioAxis->clearTraj();//保证立刻覆盖前面的旧轨迹
    speedRatioAxis->trapMove(0,1);
    basicStatus=E_WAVE_STATUS_TOCEASE;
    return 1;
}

int WaveMotion::moveOneStep(double currentVelRatio,const std::vector<double> &inialPosIn
                            , std::vector<double> &positionOut)
{
    positionOut=inialPosIn;
    speedRatioAxis->moveOneStep();
    bool isEmpty=speedRatioAxis->isTrajComposeEmpty();
    switch (basicStatus) {
    case E_WAVE_STATUS_CEASED:
    {
        return 0;
        break;
    }
    case E_WAVE_STATUS_TOCEASE:
    {
        if(isEmpty)
        {
            basicStatus=E_WAVE_STATUS_CEASED;
            return 0;
        }
        break;
    }
    case E_WAVE_STATUS_STOPED:
    {
        return 0;
        break;
    }
    case E_WAVE_STATUS_TOSTOP:
    {
        if(isEmpty)
        {
            weaveCondition.motionStyle=E_WAVE_TYPE_NONE;
            basicStatus=E_WAVE_STATUS_STOPED;
            return 0;
        }
        break;
    }
    case E_WAVE_STATUS_TORUN:
    {
        if(isEmpty)
        {
            basicStatus=E_WAVE_STATUS_RUNNING;
        }
        break;
    }
    default:
        break;
    }




    Frame originFrame(Rotation::RPY((inialPosIn[3]+weaveCondition.rotateAngle_x)*M_PI/180.0,
                      inialPosIn[4]*M_PI/180.0,
                      (inialPosIn[5]+weaveCondition.rotateAngle_z)*M_PI/180.0),
                      Vector (inialPosIn[0],
                          inialPosIn[1],
                          inialPosIn[2]));
    //改变姿态
    Frame transFrame=Frame::Identity();
    transFrame.M.DoRotX((weaveCondition.rotateAngle_x)*M_PI/180.0);
    transFrame.M.DoRotZ((weaveCondition.rotateAngle_z)*M_PI/180.0);
    Frame targetPointFrame=originFrame*transFrame;

    Frame newFrame;

    sinMove(speedRatioAxis->positionCommand,(E_WAVE_TYPE)weaveCondition.motionStyle,targetPointFrame,newFrame);
    positionOut[0]=newFrame.p[0];
    positionOut[1]=newFrame.p[1];
    positionOut[2]=newFrame.p[2];


    return 1;
}

int WaveMotion::resetAll()
{
    speedRatioAxis->clearTraj();
    weaveCondition.motionStyle=E_WAVE_TYPE_NONE;
    basicStatus=E_WAVE_STATUS_STOPED;
    return 1;
}


int WaveMotion::sinMove(double widthIn, E_WAVE_TYPE waveTypeIn,
                        const Frame &inialFrameIn, Frame &positionFrameOut)
{
    positionFrameOut=inialFrameIn;

    //停留逻辑
    switch(haltStage)
    {
    case 0:
    {
        accumulatedTime+=deltaTime;
//        accumulatedTime+=deltaTime*timeAxisScale;//可以降低按钮操作启动和停止的加速度
        if(E_WAVE_TYPE_SIN_X==waveTypeIn || E_WAVE_TYPE_SIN_Y==waveTypeIn)
        {
            if(weaveCondition.isHaltAtEdge && widthIn==weaveCondition.width/2.0)
            {
                int axisCount=theaAxisTime/(2*M_PI);
                double tmpPhaseValue=theaAxisTime-axisCount*2*M_PI;
                if(tmpPhaseValue>M_PI_2 && tmpPhaseValue<M_PI && stopCount_left!=axisCount)
                {
                    haltStage=1;
                    stopCount_left=axisCount;
                    haltTimeCount=0;
                }
                else if(tmpPhaseValue>M_PI+M_PI_2 && tmpPhaseValue<2*M_PI && stopCount_right!=axisCount)
                {
                    haltStage=2;
                    stopCount_right=axisCount;
                    haltTimeCount=0;

                }
            }

        }
        break;
    }
    case 1:
    {
        haltTimeCount+=deltaTime;
        if(haltTimeCount>weaveCondition.leftHaltTime)
        {
            haltStage=0;
        }
        break;
    }
    case 2:
    {
        haltTimeCount+=deltaTime;
        if(haltTimeCount>weaveCondition.rightHaltTime)
        {
            haltStage=0;
        }
        break;
    }
    }


    double xMove=0;
    double yMove=0;
    int phase;

    theaAxisTime=weaveCondition.frequency*2*M_PI*accumulatedTime;

    switch (waveTypeIn)
    {
    case E_WAVE_TYPE_SIN_X:
        xMove=widthIn*sin(theaAxisTime);
//        qDebug()<<"xMove"<<xMove;
        break;
    case E_WAVE_TYPE_SIN_Y:
        yMove=widthIn*sin(theaAxisTime);
        break;
    case E_WAVE_TYPE_CIRCLE_P:
        xMove=widthIn*sin(theaAxisTime);
        yMove=widthIn*cos(theaAxisTime);
        break;
    case E_WAVE_TYPE_CIRCLE_N:
        xMove=widthIn*sin(theaAxisTime);
        yMove=-widthIn*cos(theaAxisTime);
        break;
    case E_WAVE_TYPE_DOUBLECIRCLE:
    {
        phase=theaAxisTime/(2*M_PI);
        phase%=2;
        if(0==phase)
        {
            xMove=widthIn/2.0*sin(theaAxisTime-M_PI_2)+widthIn/2.0;
            yMove=-widthIn/2.0*cos(theaAxisTime-M_PI_2);
        }
        else
        {
            xMove=-widthIn/2.0*sin(theaAxisTime-M_PI_2)-widthIn/2.0;
            yMove=-widthIn/2.0*cos(theaAxisTime-M_PI_2);
        }
        break;
    }
    case E_WAVE_TYPE_DOUBLECIRCLE_Y:
    {
        phase=theaAxisTime/(2*M_PI);
        phase%=2;
        if(0==phase)
        {
            yMove=widthIn/2.0*sin(theaAxisTime-M_PI_2)+widthIn/2.0;
            xMove=-widthIn/2.0*cos(theaAxisTime-M_PI_2);
        }
        else
        {
            yMove=-widthIn/2.0*sin(theaAxisTime-M_PI_2)-widthIn/2.0;
            xMove=-widthIn/2.0*cos(theaAxisTime-M_PI_2);
        }
        break;
    }

    default:
    {

        return 0;
    }
    }

    //工具坐标系平移
    Frame transFrame=Frame::Identity();
    transFrame.p[0]=xMove;
    transFrame.p[1]=yMove;
    positionFrameOut=inialFrameIn*transFrame;

//    qDebug()<<"WaveMotion accumulatedTime"<<accumulatedTime<<"xMove"<<xMove
//              <<"yMove"<<yMove<<"widthIn"<<widthIn<<"phase"<<phase<<"tmpThea"<<tmpThea;
    return 1;

}
